package com.example.myapplication.utils;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;

import androidx.appcompat.app.AppCompatActivity;

import java.util.ArrayList;
import java.util.List;

public class MagneticUtil implements SensorEventListener {

    private Sensor accelerometer;
    private SensorManager sensorManager;
    private Sensor sensor;
    private float[] magneticValues;
    private float[] accelerometerValues;
    private float[] rotationMatrix = new float[9];
    private float[] worldCoordinates = new float[3];
    private float[] angle = new float[3];
    private float[] load = new float[3];

    public MagneticUtil(Context context) {
        sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
        sensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        accelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
    }

    public void start() {
        sensorManager.registerListener(this, sensor, SensorManager.SENSOR_DELAY_NORMAL);
        sensorManager.registerListener(this, accelerometer, SensorManager.SENSOR_DELAY_NORMAL);
    }

    public void stop() {
        sensorManager.unregisterListener(this, sensor);
        sensorManager.unregisterListener(this, accelerometer);
    }

    @Override
    public void onSensorChanged(SensorEvent event) {
        switch (event.sensor.getType()) {
            case Sensor.TYPE_MAGNETIC_FIELD:
                magneticValues = event.values.clone();
                break;
            case Sensor.TYPE_ACCELEROMETER:
                accelerometerValues = event.values.clone();
                break;
        }
        if (magneticValues != null && accelerometerValues != null) {
            convertToWorldCoordinates();
            if (accelerometerValues != null && magneticValues != null) {
                // 在这里执行其他操作或通知其他类数据更新
            }
        }
    }




    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {
//        sensorManager= (SensorManager) getSystemService(Context.SENSOR_SERVICE);
//        sensor=sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
//        accelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);

    }

    private void convertToWorldCoordinates() {
        SensorManager.getRotationMatrix(rotationMatrix, null, accelerometerValues, magneticValues);
        SensorManager.getOrientation(rotationMatrix, angle);
        convertToEarthCoordinates(magneticValues,rotationMatrix);
        matrixCalculate();
    }


    private void matrixCalculate(){

//        for(int i = 0,j=0; i < 9; i+=3,j++)
//        {
//            worldCoordinates[j] = rotationMatrix[i]*magneticValues[0] + rotationMatrix[i+1]*magneticValues[1]+rotationMatrix[i+2]*magneticValues[2];
//        }
        float[] trans_ang = {1,0,0,0,(float)Math.cos(Math.toDegrees(angle[0])),(float)(-1.0*Math.sin(Math.toDegrees(angle[0]))),0,(float)Math.sin(Math.toDegrees(angle[0])),(float)Math.cos(Math.toDegrees(angle[0]))};
        float[] pitching_ang = {(float)Math.cos(Math.toDegrees(angle[1])),0,(float)(-1.0*Math.sin(Math.toDegrees(angle[1]))),0,1,0,(float)Math.sin(Math.toDegrees(angle[1])),0,(float)Math.cos(Math.toDegrees(angle[1]))};
        float[] drift_ang = {(float)Math.cos(Math.toDegrees(angle[2])),(float)(-1.0*Math.sin(Math.toDegrees(angle[2]))),0,(float)Math.sin(Math.toDegrees(angle[1])),(float)Math.cos(Math.toDegrees(angle[2])),0,0,0,1};
        float[] temp_1 = new float[9];
        float[] temp_2 = new float[9];
        for(int i = 0,k = 0;i<9;i+=3)
        {
            for(int j = 0; j < 3; j++)
            {
                temp_1[k] = trans_ang[i]*pitching_ang[j]+trans_ang[i+1]*pitching_ang[j+3]+trans_ang[i+2]*pitching_ang[j+6];
                k++;
            }
        }

        for(int i = 0,k = 0;i<9;i+=3)
        {
            for(int j = 0; j < 3; j++)
            {
                temp_2[k] = temp_1[i]*drift_ang[j]+temp_1[i+1]*drift_ang[j+3]+temp_1[i+2]*drift_ang[j+6];
                k++;
            }
        }

        for(int i = 0,j=0; i < 9; i+=3,j++)
        {
            load[j] = temp_2[i]*worldCoordinates[0] + temp_2[i+1]*worldCoordinates[1]+temp_2[i+2]*worldCoordinates[2];
        }
    }

    private void convertToEarthCoordinates(float[] magneticData, float[] rotationMatrix) {
        // 应用旋转矩阵，将地磁坐标转换为世界坐标系
        worldCoordinates[0] = rotationMatrix[0] * magneticData[0] +
                rotationMatrix[1] * magneticData[1] +
                rotationMatrix[2] * magneticData[2];

        worldCoordinates[1] = rotationMatrix[3] * magneticData[0] +
                rotationMatrix[4] * magneticData[1] +
                rotationMatrix[5] * magneticData[2];

        worldCoordinates[2] = rotationMatrix[6] * magneticData[0] +
                rotationMatrix[7] * magneticData[1] +
                rotationMatrix[8] * magneticData[2];
    }

    public float[] globalValues(){
        return worldCoordinates;
    }
}
